function [D,X,Z] = BeamForming(datos,d,rsf,c,ti,ResX,ResZ)

%datos: eso, los recolectados por los sensores
%d: distancia entre sensores
%rsf: frecuencia en MHZ
%c: velocidad de propagación en m/s
%z1: profundidad en la que se empieza a sensar
%zf: profundidad en la que se termina
%resX: resolución deseada en X
%resZ: resolución deseada en Y


fs=rsf*10^6; %Paso de MHz a Hz
[M,N]=size(datos);
if nargin<6
    ResX=N;
    ResZ=1024;
end
if nargin<5
    ti=0;
end
if (nargin<4)
    c=1500;
end
    
    
matlabpool size;
if (ans==0)
    matlabpool open 4
end
tic

zi=ti*c;
 zf=(c/fs)*M;
 z1=0;
minX=0;
maxX=N*d;
minZ=z1+zi;
maxZ=zf+zi;
DeltaZ=maxZ-minZ;
DeltaX=maxX-minX;
dZ=DeltaZ/ResZ;
dX=DeltaX/ResX;


ret_min=floor((minZ/c)*fs); %Adimensionado
ret_max=floor((maxZ/c)*fs); %Adimensionado
zeros_prev=round(N*d/c * fs);
zeros_post=zeros_prev;
A=zeros(ResX,ResZ,M+zeros_prev+zeros_post+ret_max-ret_min);
D=zeros(ResX,ResZ);

%Do zero padding
newM=M+zeros_prev+zeros_post;
newM=2^(ceil(log(newM)/log(2)));
data=zeros(newM,N);
data(zeros_prev+1:zeros_prev+M,:)=datos;
%Calculate FFT to avoid computation in each step of the iteration
Fdata=fft(data);

max_retardo=floor(fs*N*d/c);
parfor i=1:ResX
    B=zeros(1,ResZ);
    tic
    i
    x=minX+(i-1)*dX;
        for j=1:ResZ
            z=minZ + (j-1)* dZ;
            dist=sqrt(x^2 + z^2);
            mean_retardo=zeros_prev+ceil(fs*(dist/c - ti)); %Esto tengo que mejorarlo
            %t1=max(mean_retardo-max_retardo,1); 
            %t2=min(mean_retardo+max_retardo,M); 
            %salida=BeamForm1(data(t1:t2,:),x,z,c,d);
            %muestras_ret=mean_retardo-t1+1;
            
            salida=BeamForm1(data,x,z,c,d,fs,Fdata); %Para video
            muestras_ret=mean_retardo;
            %%Interpolacion lineal
            %m1=floor(muestras_ret);
            %m2=m1+1;
            %aux=salida(m1) * (muestras_ret-m1) + salida(m2) * (m2-muestras_ret);
            %A(i,j)=aux;
            %A(i,j,ret_max-muestras_ret+1:ret_max-muestras_ret+newM)=salida;
            
            %Me quedo solo con la foto del instante t=0
            B(j)=salida(muestras_ret);
        end
     D(i,:)=real(B);   
     toc
end
Z=minZ+[0:(ResZ-1)]*dZ;
X=minX+[0:(ResX-1)]*dX;
matlabpool close
end

function y=BeamForm1(data,x,z,c,d,fs,Fdata)

%data: matriz de datos, N sensores x M muestras en el tiempo
%x: distancia de focaliz. sobre el eje del array. 0 coincide con el centro del primer sensor.
%z: profundidad de focaliz.

if nargin<6
    Fdata=fft(data); %FFT por columnas (por sensor)
end

N=size(data,2);



rx=[[0:(N-1)]*d - x]';

modR=sqrt(rx.*rx + z^2); %Vector columna
fase=2*pi*fs*(modR-modR(1))'/c;
M=size(Fdata,1);

%f= fftshift([(-M/2):1:(M/2 -1)]/M)'; %Vector columna
%aux=f*fase;
%Wb=pol2cart(aux,1);
%otra forma
f= [1:1:(M/2)]'/M; %Vector columna
aux=f*fase;
Wb=ones(M,N);
Wb(2:(M/2+1),:)=cos(aux)+i*sin(aux);
Wb(M/2+1,:)=cos(aux(end,:));
Wb((M/2+1):M,:)=conj(Wb([(M/2+1):-1:2],:));

%W= Wa .* Wb;
%YFpredata=Fdata .* W;
%YF = sum(YFpredata,2); %Sumo sobre la dimension correcta, espero
%Otro approach
YFpredata=Fdata.*Wb;
YF=YFpredata*modR;

y=ifft(YF);
end